using BepuPhysics.Collidables;
using BepuUtilities;
using BepuUtilities.Memory;
using System.Diagnostics;
using System.Runtime.CompilerServices;
using static BepuUtilities.GatherScatter;
#if MYCODE
using BepuUtilities.Vectors;
#else
using System.Numerics;
#endif
namespace BepuPhysics.CollisionDetection.CollisionTasks
{
    public interface IPairTester<TShapeWideA, TShapeWideB, TManifoldWideType>
    {
        /// <summary>
        /// 获取在执行广泛测试之前理想情况下收集在一起的对的数量。
        /// </summary>
        int BatchSize { get; }

        // 请注意,虽然接口需要所有这三个实现,但具体的实现者只会定义或调用一个。
        // 这里包括其他未使用的函数只是为了简化其在批处理执行循环中的使用。
        void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationA, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold);
        void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, ref QuaternionWide orientationB, int pairCount, out TManifoldWideType manifold);
        void Test(ref TShapeWideA a, ref TShapeWideB b, ref Vector<float> speculativeMargin, ref Vector3Wide offsetB, int pairCount, out TManifoldWideType manifold);
    }

    public interface IContactManifoldWide
    {
        void ApplyFlipMask(ref Vector3Wide offsetB, in Vector<int> flipMask);
        void ReadFirst(in Vector3Wide offsetB, ref ConvexContactManifold target);
    }

    public class ConvexCollisionTask<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPair, TPairWide, TManifoldWide, TPairTester> : CollisionTask
            where TShapeA : unmanaged, IShape where TShapeB : unmanaged, IShape
            where TShapeWideA : unmanaged, IShapeWide<TShapeA> where TShapeWideB : unmanaged, IShapeWide<TShapeB>
            where TPair : unmanaged, ICollisionPair<TPair>
            where TPairWide : unmanaged, ICollisionPairWide<TShapeA, TShapeWideA, TShapeB, TShapeWideB, TPair, TPairWide>
            where TPairTester : struct, IPairTester<TShapeWideA, TShapeWideB, TManifoldWide>
            where TManifoldWide : unmanaged, IContactManifoldWide
    {
        public ConvexCollisionTask()
        {
            BatchSize = default(TPairTester).BatchSize;
            ShapeTypeIndexA = default(TShapeA).TypeId;
            ShapeTypeIndexB = default(TShapeB).TypeId;
            PairType = default(TPair).PairType;
        }

        [MethodImpl(MethodImplOptions.AggressiveInlining)]
        public override unsafe void ExecuteBatch<TCallbacks>(ref UntypedList batch, ref CollisionBatcher<TCallbacks> batcher)
        {
            ref var start = ref Unsafe.As<byte, TPair>(ref batch.Buffer[0]);
            // 如果运气好的话,编译器最终会去掉这些不必要的零初始值。
            // 也许可以用一些巨型攻击来消除exploldWide和defaultPairTester,但是它有很大的前进危险和值得怀疑的好处。
            var pairWide = default(TPairWide);
            ref var aWide = ref pairWide.GetShapeA(ref pairWide);
            ref var bWide = ref pairWide.GetShapeB(ref pairWide);
            if (aWide.InternalAllocationSize > 0)
            {
                var memory = stackalloc byte[aWide.InternalAllocationSize];
                aWide.Initialize(new RawBuffer(memory, aWide.InternalAllocationSize));
            }
            if (bWide.InternalAllocationSize > 0)
            {
                var memory = stackalloc byte[bWide.InternalAllocationSize];
                bWide.Initialize(new RawBuffer(memory, bWide.InternalAllocationSize));
            }
            TManifoldWide manifoldWide;
            var defaultPairTester = default(TPairTester);
            var manifold = default(ConvexContactManifold);

            for (int i = 0; i < batch.Count; i += Vector<float>.Count)
            {
                ref var bundleStart = ref Unsafe.Add(ref start, i);
                int countInBundle = batch.Count - i;
                if (countInBundle > Vector<float>.Count)
                    countInBundle = Vector<float>.Count;

                // TODO：如果我们有收集本质,他们可能会赢。
                for (int j = 0; j < countInBundle; ++j)
                {
                    pairWide.WriteSlot(j, Unsafe.Add(ref bundleStart, j));
                }

                if (pairWide.OrientationCount == 2)
                {
                    defaultPairTester.Test(
                        ref aWide,
                        ref bWide,
                        ref pairWide.GetSpeculativeMargin(ref pairWide),
                        ref pairWide.GetOffsetB(ref pairWide),
                        ref pairWide.GetOrientationA(ref pairWide),
                        ref pairWide.GetOrientationB(ref pairWide),
                        countInBundle,
                        out manifoldWide);
                }
                else if (pairWide.OrientationCount == 1)
                {
                    // 请注意,如果只有一个方向,则它属于第二个形状。
                    // 唯一不需要定向的形状是球体,按照惯例,它将位于插槽A中。
                    Debug.Assert(typeof(TShapeWideA) == typeof(SphereWide));
                    defaultPairTester.Test(
                        ref aWide,
                        ref bWide,
                        ref pairWide.GetSpeculativeMargin(ref pairWide),
                        ref pairWide.GetOffsetB(ref pairWide),
                        ref pairWide.GetOrientationB(ref pairWide),
                        countInBundle,
                        out manifoldWide);
                }
                else
                {
                    Debug.Assert(pairWide.OrientationCount == 0);
                    Debug.Assert(typeof(TShapeWideA) == typeof(SphereWide) && typeof(TShapeWideB) == typeof(SphereWide), "No orientation implies a special case involving two spheres.");
                    // 真的,这是可以直接作为特例的,但是呢。
                    defaultPairTester.Test(
                        ref aWide,
                        ref bWide,
                        ref pairWide.GetSpeculativeMargin(ref pairWide),
                        ref pairWide.GetOffsetB(ref pairWide),
                        countInBundle,
                        out manifoldWide);
                }

                // 翻转与必须翻转形状顺序的配对相关联的任何触点。
                if (pairWide.HasFlipMask)
                {
                    manifoldWide.ApplyFlipMask(ref pairWide.GetOffsetB(ref pairWide), pairWide.GetFlipMask(ref pairWide));
                }

                for (int j = 0; j < countInBundle; ++j)
                {
                    ref var manifoldSource = ref GetOffsetInstance(ref manifoldWide, j);
                    ref var offsetSource = ref GetOffsetInstance(ref pairWide.GetOffsetB(ref pairWide), j);
                    manifoldSource.ReadFirst(offsetSource, ref manifold);
                    ref var pair = ref Unsafe.Add(ref bundleStart, j);
                    batcher.ProcessConvexResult(ref manifold, ref pair.GetContinuation(ref pair));
                }
            }

        }

    }
}
